#include <Geom_Line.hxx>
#include <Geom_Circle.hxx>
#include <Geom_BSplineCurve.hxx>
#include <BRep_Tool.hxx>
#include <ShapeAnalysis_Edge.hxx>
#include <TopoDS_Vertex.hxx>
#include <gp_Circ.hxx>
#include <gp_Pln.hxx>
#include <BRepBuilderAPI_MakeEdge.hxx>
#include <ShapeFix_ShapeTolerance.hxx>
#include <Geom_TrimmedCurve.hxx>

#include "EntFromEdge.h"
#include "LineStrip2D.h"
#include "Arc2D.h"
#include "Spline2D.h"
#include "Plane.h"
#include "LineStrip3D.h"
#include "Arc3D.h"
#include "Spline3D.h"
#include "Ucs.h"
#include "OcctMakeShape.h"
#include "OcctMisc.h"

std::vector<osg::Vec3d> convert(const TColgp_Array1OfPnt& points);
std::vector<double> convert(const TColStd_Array1OfReal& weights);
std::vector<int> convert(const TColStd_Array1OfInteger& mults);
void toACADKnots(const std::vector<double>& knots, const std::vector<int>& mults, std::vector<double>& acadKnots);

std::shared_ptr<Ent2D> makeEnt2D(const TopoDS_Edge & edge, INode* parent)
{
	double First, Last;
	const Handle(Geom_Curve)& c = BRep_Tool::Curve(edge, First, Last);
	if (c->IsKind(STANDARD_TYPE(Geom_Line)))
	{
		ShapeAnalysis_Edge anEdgeAnalyser;
		TopoDS_Vertex firstVertex = anEdgeAnalyser.FirstVertex(edge);
		TopoDS_Vertex lastVertex = anEdgeAnalyser.LastVertex(edge);
		gp_Pnt firstPnt = BRep_Tool::Pnt(firstVertex);
		gp_Pnt lastPnt = BRep_Tool::Pnt(lastVertex);
		auto line = std::make_shared<LineStrip2D>(parent);
		std::vector<osg::Vec3d> points;
		points.push_back(osg::Vec3d(firstPnt.X(), firstPnt.Y(), firstPnt.Z()));
		points.push_back(osg::Vec3d(lastPnt.X(), lastPnt.Y(), lastPnt.Z()));
		line->setPoints(points);
		return line;
	}
	else if (c->IsKind(STANDARD_TYPE(Geom_Circle)))
	{
		auto curve = Handle(Geom_Circle)::DownCast(c.get());
		auto circ = curve->Circ();
		double radius = circ.Radius();
		gp_Pnt circCenter;
		osg::Vec3d center(circCenter.X(), circCenter.Y(), circCenter.Z());
		auto ax2 = circ.Position();
		Plane cirPlane;
		auto origin = ax2.Location();
		auto xDir = ax2.XDirection();
		auto yDir = ax2.YDirection();
		cirPlane.setOrigin(osg::Vec3d(origin.X(), origin.Y(), origin.Z()));
		cirPlane.setAxis(osg::Vec3d(xDir.X(), xDir.Y(), xDir.Z()), osg::Vec3d(yDir.X(), yDir.Y(), yDir.Z()));
		osg::Matrix matrix = cirPlane.getMatrix();
		center = center * matrix;
		osg::Vec3d firstDir(cos(First), sin(First), 0);
		auto firstDir2 = osg::Matrix::transform3x3(firstDir, matrix);
		auto firstAngle = atan2(firstDir2.y(), firstDir2.x());
		osg::Vec3d lastDir(cos(Last), sin(Last), 0);
		auto lastDir2 = osg::Matrix::transform3x3(lastDir, matrix);
		auto lastAngle = atan2(lastDir2.y(), lastDir2.x());
		auto zDirPnt = ax2.Direction();
		osg::Vec3d zDir(zDirPnt.X(), zDirPnt.Y(), zDirPnt.Z());
		auto dotProduct = zDir * osg::Vec3d(0, 0, 1);
		if (dotProduct < 0)
			std::swap(firstAngle, lastAngle);
		std::shared_ptr<Arc2D> filletArc = std::make_shared<Arc2D>(parent);
		// 把大圆改成互补的小圆
		if (lastAngle - firstAngle<0)
		{
			lastAngle = lastAngle + 2 * M_PI;
		}
		if (lastAngle - firstAngle > M_PI)
		{
			lastAngle = lastAngle - 2 * M_PI;
			std::swap(firstAngle, lastAngle);
		}
		filletArc->set(center, radius, firstAngle, lastAngle);
		return filletArc;
	}
	else if (c->IsKind(STANDARD_TYPE(Geom_BSplineCurve)))
	{
		auto curve = Handle(Geom_BSplineCurve)::DownCast(c.get());
		int m_Flag;
		int m_Degree;
		std::vector<double> acadKnots;
		std::vector<double> m_Weights;
		std::vector<osg::Vec3d> m_ControlPoints;
		m_Flag = curve->IsPeriodic() ? 2 : 0;
		m_Flag |= 8; // 平面
		m_Degree = curve->Degree();
		auto poles = curve->Poles();
		m_ControlPoints = convert(poles);
		auto weights = curve->Weights();
		if (weights)
			m_Weights = convert(*weights);
		else
			m_Weights.resize(m_ControlPoints.size(), 1.0);
		auto knots = curve->Knots();
		auto mults = curve->Multiplicities();
		acadKnots.clear();
		toACADKnots(convert(knots), convert(mults), acadKnots);

		auto spline = std::make_shared<Spline2D>(parent);
		spline->setDegree(m_Degree);
		spline->setFlag(m_Flag);
		spline->setCtrlPoints(m_ControlPoints);
		spline->setWeights(m_Weights);
		spline->setKnots(acadKnots);
		return spline;
	}
	return std::shared_ptr<Ent2D>();
}

std::vector<TopoDS_Edge> makeEdges(const std::shared_ptr<Ent>& ent)
{
	std::vector<Handle(Geom_Curve)> curve3ds;
	auto ent2d = dynamic_cast<Ent2DCurve*>(ent.get());
	if (ent2d)
	{
		auto curve2ds = ent2d->getCurve2d();
		Handle(Geom2d_Curve) first = curve2ds[0];
		auto ucs = dynamic_cast<UCS*>(ent2d->getParent());
		gp_Pln plane = ucs->getPln();

		for (const auto& curve2d : curve2ds)
		{
			Handle(Geom_Curve) curve3d = curves2dTo3d(curve2d, plane);
			curve3ds.push_back(new Geom_TrimmedCurve(curve3d, curve3d->FirstParameter(), curve3d->LastParameter()));
		}
	}
	else
	{
		auto ent3d = dynamic_cast<Ent3DCurve*>(ent.get());
		if (ent3d)
			curve3ds = ent3d->getCurve3d();
	}
	std::vector<TopoDS_Edge> result;
	for (const auto& curve3d : curve3ds)
	{
		auto edge = makeEdge(curve3d, curve3d->FirstParameter(), curve3d->LastParameter());
		result.push_back(edge);
	}
	return result;
}

std::shared_ptr<Ent3D> makeEnt3DFromEnt2D(const std::shared_ptr<Ent2D> & ent2D,
	INode* parent, const osg::Matrixd& matrix)
{
	osg::Vec3d xAxis = { 1,0,0 };
	osg::Vec3d yAxis = { 0,1,0 };
	osg::Vec3d origin = matrix.getTrans();
	xAxis = osg::Matrix::transform3x3(xAxis, matrix);
	yAxis = osg::Matrix::transform3x3(yAxis, matrix);
	if (ent2D->metaObject() == &LineStrip2D::staticMetaObject)
	{
		auto line2d = std::static_pointer_cast<LineStrip2D>(ent2D);
		auto points = line2d->getPoints();
		for (auto& pt : points)
		{
			pt = pt * matrix;
		}
		auto line3d = std::make_shared<LineStrip3D>(parent);
		line3d->setPoints(points);
		return line3d;
	}
	else if (ent2D->metaObject() == &Arc2D::staticMetaObject)
	{
		auto arc2d = std::static_pointer_cast<Arc2D>(ent2D);
		auto arc3d = std::make_shared<Arc3D>(parent);
		arc3d->setAxis(xAxis, yAxis);
		arc3d->setOrigin(origin);
		arc3d->set(arc2d->center(), arc2d->radius(), arc2d->startAngle(), arc2d->endAngle());
		return arc3d;
	}
	else if (ent2D->metaObject() == &Spline2D::staticMetaObject)
	{
		auto spline2d = std::static_pointer_cast<Spline2D>(ent2D);
		auto controlPts = spline2d->getCtrlPoints();
		for (auto& pt : controlPts)
		{
			pt = pt * matrix;
		}
		auto spline3d = std::make_shared<Spline3D>(parent);
		spline3d->setDegree(spline2d->getDegree());
		spline3d->setFlag(spline2d->getFlag());
		spline3d->setCtrlPoints(controlPts);
		spline3d->setWeights(spline2d->getWeights());
		spline3d->setKnots(spline2d->getKnots());
		return spline3d;
	}
	return std::shared_ptr<Ent3D>();
}
